#!/usr/bin/env python3

import subprocess
import time
import os
import argparse
from manager import ManagerNode
import rospy
import json
import signal
import rosgraph
from WebsocketsAPI import WebsocketsAPI


class ROSRunner:
    """Class to manage and run ROS-related processes."""

    def __init__(self, args):
        self.args = args

    def start_roscore(self):
        """Start roscore in the background."""
        print("Starting roscore...")
        if self.check_roscore():
            print("roscore is already running.")
            return None
        roscore_proc = subprocess.Popen(
            ["roscore"], stdout=subprocess.PIPE, stderr=subprocess.PIPE
        )
        return roscore_proc

    def start_video_preview_page(self):
        """Start the video preview page."""
        print("Starting video preview page...")
        video_folder = self.get_abs_path(self.args.record_output_dir)
        ip_address = self.get_page_ip()
        cmd = f"python3 app.py {video_folder} {ip_address}"
        video_preview_proc = subprocess.Popen(cmd, shell=True, preexec_fn=os.setsid)
        return video_preview_proc

    def get_page_ip(self):
        if self.args.page_ip is not None:
            return self.args.page_ip
        settings_file = self.get_abs_path(self.args.settings_file)
        settings = json.load(open(settings_file))
        return settings["LocalHostIp"]

    def start_ros_node(self):
        """Start the ROS node (record_node) with the provided arguments."""
        print("Starting ROS node...")

        rospy.init_node("simulator_manager_node")
        manager_node = ManagerNode(args)
        return manager_node

    def start_websocket_api(self):
        """Start the websocket API."""
        print("Starting websocket API...")
        websocketapi = WebsocketsAPI(self.get_abs_path(self.args.record_output_dir))
        websocketapi.start()
        return websocketapi

    def simulator_script(self):
        """Start the simulator script (circle_obstacle.sh)."""
        print("Starting simulator script...")
        simulator_path = self.get_abs_path(self.args.simulator_script)
        settings_path = self.get_abs_path(self.args.settings_file)
        simulator_proc = subprocess.Popen(
            [
                simulator_path,
                f"-settings={settings_path}",
                f"-ResX={self.args.resX}",
                f"-ResY={self.args.resY}",
                "-windowed",
            ]
        )
        return simulator_proc

    def stop_roscore(self, roscore_proc):
        """Stop roscore."""
        print("Stopping roscore...")
        roscore_proc.terminate()
        roscore_proc.wait()

    def stop_ros_node(self, ros_node_proc):
        """Stop the ROS node."""
        print("Stopping ROS node...")
        ros_node_proc.stop()

    def stop_simulator_script(self, simulator_proc):
        """Stop the simulator script."""
        print("Stopping simulator script...")
        simulator_proc.terminate()
        simulator_proc.wait()

    def stop_video_preview_page(self, video_preview_proc):
        """Stop the video preview page."""
        print("Stopping video preview page...")
        os.killpg(os.getpgid(video_preview_proc.pid), signal.SIGTERM)
        video_preview_proc.wait()

    def stop_websocket_api(self, websocketapi):
        """Stop the websocket API."""
        print("Stopping websocket API...")
        websocketapi.stop()
        websocketapi.join()

    def kill_all_rosnodes(self):
        """Kill all ROS nodes."""
        print("Killing all ROS nodes...")
        subprocess.call(["rosnode", "kill", "-a"])

    def run(self):
        """Run all processes with the given configuration."""
        try:
            video_preview_proc = self.start_video_preview_page()
            simulator_proc = self.simulator_script()
            time.sleep(5)
            # Start roscore
            roscore_proc = self.start_roscore()
            websocketapi = self.start_websocket_api()
            time.sleep(3)
            # Start ROS node
            manager_node = self.start_ros_node()
            websocketapi.set_manager_ros(manager_node)

            while simulator_proc.poll() is None:
                rospy.sleep(1)

        except KeyboardInterrupt:
            print("Process interrupted. Shutting down...")

        finally:
            # Stop all processes
            if video_preview_proc is not None:
                self.stop_video_preview_page(video_preview_proc)
            if manager_node is not None:
                self.stop_ros_node(manager_node)
            if simulator_proc is not None:
                self.stop_simulator_script(simulator_proc)
            if roscore_proc is not None:
                self.stop_roscore(roscore_proc)
            if websocketapi is not None:
                self.stop_websocket_api(websocketapi)
            print("All processes stopped.")
            exit(0)

    def check_roscore(self):
        if rosgraph.is_master_online():
            return True
        else:
            return False

    def get_abs_path(self, path):
        if os.path.isabs(path):
            return path
        """Get absolute path for the given path."""
        return os.path.join(self.args.base_path, path)


def parse_args():
    """Parse command-line arguments."""
    parser = argparse.ArgumentParser(
        description="Run ROS and simulator scripts with custom configurations."
    )
    parser.add_argument(
        "--base-path",
        type=str,
        default=".",
        help="Base path for the simulator script.",
    )

    parser.add_argument(
        "--window-title",
        type=str,
        default=None,
        help="Title of the window to record.",
    )
    parser.add_argument(
        "--record-output-dir",
        type=str,
        default="recordings",
        help="Directory for output recordings.",
    )
    parser.add_argument(
        "--rec-trigger-topic",
        type=str,
        default="/mavros/imu/data",
        help="Topic for record trigger. Can be any topic.",
    )
    parser.add_argument(
        "--lap-count-topic",
        type=str,
        default="/lap_count",
        help="Topic to publish circle count.",
    )
    parser.add_argument(
        "--timer-duration",
        type=float,
        default=0.1,
        help="Duration of timer callback in seconds.",
    )
    parser.add_argument(
        "--min-overlap-dist",
        type=float,
        default=0.2,
        help="Minimum distance for overlap detection.",
    )
    parser.add_argument(
        "--start-trigger-box",
        type=str,
        default=None,
        help="Name of the start trigger box.",
    )
    parser.add_argument(
        "--end-trigger-box",
        type=str,
        default=None,
        help="Name of the end trigger box.",
    )
    parser.add_argument(
        "--settings-file",
        type=str,
        default="settings.json",
        help="Settings file for simulator script.",
    )
    parser.add_argument(
        "--simulator-script",
        type=str,
        help="Simulator shell script to run.",
    )
    parser.add_argument(
        "--scene-config-file",
        type=str,
        default=None,
        help="Scene configuration file for the simulator script.",
    )
    parser.add_argument(
        "--resX", type=int, default=640, help="Resolution X for the simulator window."
    )
    parser.add_argument(
        "--resY", type=int, default=480, help="Resolution Y for the simulator window."
    )
    parser.add_argument(
        "--auto-reset-airsim", action="store_true", help="Auto reset AirSim."
    )
    parser.add_argument(
        "--reset-topic", type=str, default="/auto_test/reset", help="Reset topic."
    )
    parser.add_argument(
        "--collision-topic",
        type=str,
        default="/scene_manager/collision",
        help="Collision topic.",
    )
    parser.add_argument(
        "--ply-gen-xrange",
        type=float,
        nargs=2,
        default=[-3, 5],
    )
    parser.add_argument(
        "--ply-gen-yrange",
        type=float,
        nargs=2,
        default=[-2.5, 2.5],
    )
    parser.add_argument(
        "--ply-gen-zrange",
        type=float,
        nargs=2,
        default=[0, 4],
    )
    parser.add_argument(
        "--page-ip",
        type=str,
        default=None,
        help="IP address for the video preview page.",
    )
    parser.add_argument(
        "--actors-to-pub",
        nargs="+",
        type=str,
        default=[],
        help="List of actors to publish",
    )
    return parser.parse_args()


if __name__ == "__main__":
    # Parse arguments from command line
    args = parse_args()
    print(args.settings_file)
    # Create and run the ROSRunner
    ros_runner = ROSRunner(args)
    ros_runner.run()
